#include "Serial.h"

using namespace std;

pthread_t send_tid, recv_tid, monitor_tid;

int fd0;
int fd1;
int fd2;

int open_uart(){
    int tmp;

    char *uart0 = (char *)"/dev/ttyUSB0";
    char *uart1 = (char *)"/dev/ttyUSB1";
    char *uart2 = (char *)"/dev/ttyUSB2";

    fd0 = open(uart0, O_RDWR | O_NOCTTY | O_NDELAY);
    fd1 = open(uart1, O_RDWR | O_NOCTTY | O_NDELAY);
    fd2 = open(uart2, O_RDWR | O_NOCTTY | O_NDELAY);

    
    if(!(-1 == fd0))
    {
        printf("成功打开串口。\n");
        if (-1 == set_opt(fd0, 115200, 8, 'N', 1))
        {
            perror("配置串口失败");
            close(fd0);
            return -1;
        }
        else
            tmp = fd0;
    }
    else if(!(-1 == fd1))
    {
        printf("成功打开串口。\n");
        if (-1 == set_opt(fd1, 115200, 8, 'N', 1))
        {
            perror("配置串口失败");
            close(fd1);
            return -1;
        }
        else
            tmp = fd1;
    }
    else if(!(-1 == fd2))
    {
        printf("成功打开串口。\n");
        if (-1 == set_opt(fd2, 115200, 8, 'N', 1))
        {
            perror("配置串口失败");
            close(fd2);
            return -1;
        }
        else
            tmp = fd2;
    }
    else
    {
        perror("无法打开串口");
        return -1;
    }
    // 设置为阻塞模式
    fcntl(tmp, F_SETFL, 0);

    // 创建发送线程
    if (0 != pthread_create(&send_tid, NULL, send_thread, NULL))
    {
        perror("创建发送线程失败");
        close(tmp);
        return -1;
    }

    // 创建接收线程
    if (0 != pthread_create(&recv_tid, NULL, receive_thread, NULL))
    {
        perror("创建接收线程失败");
        close(tmp);
        return -1;
    }

    if(0 != pthread_create(&monitor_tid, NULL, monitor_thread, NULL))
    {
        perror("创建接收线程失败");
        close(tmp);
        return -1;
    }
    return tmp;
}

void *send_thread(void *arg)
{
    VisionSendStructTypedef message = {S_yaw, S_pitch, 1, 100}; // 示例数据
    unsigned char bufferW[sizeof(message) + 3];
    while (keep_running)
    {
        message.yaw = S_yaw;
        message.pitch = S_pitch;
        S_yaw = 0;
        S_pitch = 0;
        bufferW[0] = 0xFF; // 标识符
        bufferW[1] = sizeof(message);
        memcpy(bufferW + 2, &message, sizeof(message)); // 拷贝结构体数据
        bufferW[sizeof(message) + 2] = 0xFE;
        int n = write(fd, bufferW, sizeof(message) + 3);
        if (n < 0)
        {
            perror("发送数据失败");
            break;
        }
        else
        {
            printf("发送: Yaw = %f, Pitch = %f, Fire = %d, Distance = %d, end = 0x%0X\n", message.yaw, message.pitch, message.fire, message.distance, bufferW[sizeof(message)+2]);
            message.yaw = 0;
            
            fflush(stdout);
            // cout << "yaw: " << message.yaw << " " <<  "pitch: " << message.pitch << endl;
        }
        usleep(100000); // 延时
    }
    exit(-1);
    return NULL;
}

typedef enum
{
    eReNone = 0,
    eReHead,
    eReLen,
    eReData,
    eReTail,
} ERecvStatusTypedef;
#define HEAD_FRAME (0xFF)
#define TAIL_FRAME (0xFE)

VisionRecvStructTypedef g_sRecvData;

void vision_recdata_in(uint8_t data)
{
    static unsigned char buffer[sizeof(g_sRecvData)];
    static uint8_t _cvSta = 0;
    static unsigned char _opt;
    static uint8_t _lens;

    switch(_cvSta)
    {
        case eReNone:
            if(data == HEAD_FRAME)
            {
                _cvSta = eReHead;
                memset(&buffer[0], 0, sizeof(buffer));
                _opt = 0;
            }
            else
            {
                _cvSta = eReNone;
                _opt = 0;
            }
            break;
        case eReHead:
            _lens = data;
            _cvSta = eReLen;
            _opt = 0;
            break;
        case eReLen:
            buffer[_opt++] = data;
            if(_opt >= _lens)
                _cvSta = eReData;
            break;
        case eReData:
            if(data == TAIL_FRAME)
            {
                memcpy(&g_sRecvData, &buffer[0], sizeof(buffer)+1);
                // printf("yaw = %f, pitch = %f\n", g_sRecvData.yaw, g_sRecvData.pitch);
                R_yaw = g_sRecvData.yaw;
                R_pitch = g_sRecvData.pitch;
                _cvSta = eReNone;
            }
            else
            {
                _cvSta = eReNone;
                printf("\n");
            }
            break;
        case eReTail:
            _cvSta = eReNone;
            _opt = 0;
            break;
        default:
            break;
    }
}

void* receive_thread(void *arg)
{
    uint8_t buffer_rec[2];

    while(keep_running)
    {
        static int nByte;
        if(1 == nByte)
        {
            vision_recdata_in(buffer_rec[0]);
            nByte = 0;
        }
        nByte = read(fd, buffer_rec, 1);
    }
    exit(-1);
    return nullptr;
}

void* monitor_thread(void *arg)
{
    printf("按q退出程序...\n");
    while(keep_running)
    {
        printf("检测执行中\n");
        fflush(stdout);
        char c = getchar();
        if('q' == c || c == 'Q')
        {
            keep_running = 0;
            int flags = fcntl(STDIN_FILENO, F_GETFL, 0);
            fcntl(STDIN_FILENO, F_SETFL, flags | O_NONBLOCK);
            printf("程序即将退出...\n");
            break;
        }
        usleep(100000);
    }
    exit(-1);
    return nullptr;
}

int set_opt(int fd, int nSpeed, int nBits, char nEvent, int nStop)
{
    struct termios newtio, oldtio;
    if (0 != tcgetattr(fd, &oldtio))
    {
        perror("无法获取串口属性");
        return -1;
    }
    bzero(&newtio, sizeof(newtio));

    newtio.c_cflag |= CLOCAL | CREAD;

    newtio.c_cflag &= ~CSIZE;
    switch (nBits)
    {
        case 7:
            newtio.c_cflag |= CS7;
            break;
        case 8:
        default:
            newtio.c_cflag |= CS8;
            break;
    }

    switch (nEvent)
    {
        case 'O':
            newtio.c_cflag |= PARENB | PARODD;
            newtio.c_iflag |= INPCK;
            break;
        case 'E':
            newtio.c_cflag |= PARENB;
            newtio.c_cflag &= ~PARODD;
            newtio.c_iflag |= INPCK;
            break;
        case 'N':
        default:
            newtio.c_cflag &= ~PARENB;
            break;
    }

    switch (nSpeed)
    {
        case 2400:
            cfsetispeed(&newtio, B2400);
            cfsetospeed(&newtio, B2400);
            break;
        case 4800:
            cfsetispeed(&newtio, B4800);
            cfsetospeed(&newtio, B4800);
            break;
        case 9600:
            cfsetispeed(&newtio, B9600);
            cfsetospeed(&newtio, B9600);
            break;
        case 115200:
        default:
            cfsetispeed(&newtio, B115200);
            cfsetospeed(&newtio, B115200);
            break;
    }

    if (2 == nStop)
        newtio.c_cflag |= CSTOPB;
    else
        newtio.c_cflag &= ~CSTOPB;

    newtio.c_cc[VTIME] = 0;
    newtio.c_cc[VMIN] = 1;

    tcflush(fd, TCIFLUSH);
    if (0 != (tcsetattr(fd, TCSANOW, &newtio)))
    {
        perror("设置串口属性失败");
        return -1;
    }
    return 0;
}

// #pragma pack(1)
//     typedef struct
//     {
//         float serial_Yaw;
//         float serial_Pitch;
//         uint8_t fire;   // 0: 不开火, 1: 开火
//         int16_t distance;
//     } VisionRecvStructTypedef;
// #pragma pack()

// Serial::Serial(int nSpeed, int nBits, char nEvent, int nStop)
// {

// }

// Serial::~Serial()
// {
//     closeSerial();
// }

// void Serial::closeSerial()
// {
//     if(1 == close0)
//     {
//         keep_running = false;
//         close(fd);
//         fd = -1;
//     }
// }

// void Serial::autoSerial()
// {
//     serialPreprocessor();
// }

// void Serial::serialPreprocessor()
// {
//     char *uart0 = (char *)"/dev/ttyUSB0";
//     pthread_t send_tid, recv_tid, monitor_tid;

//     fd = open(uart0, O_RDWR | O_NOCTTY | O_NDELAY);
//     if (fd == -1)
//     {
//         perror("无法打开串口");
//     }
//     else
//     {
//         printf("成功打开串口。\n");
//         if (set_opt(fd, 115200, 8, 'N', 1) == -1)
//         {
//             perror("配置串口失败");
//             close(fd);
//         }
//     }

//     // 设置为阻塞模式
//     fcntl(fd, F_SETFL, 0);

//     // 创建发送线程
//     if (pthread_create(&send_tid, NULL, start_send_thread, NULL) != 0)
//     {
//         perror("创建发送线程失败");
//         close(fd);
//     }

//     // 创建接收线程
//     // if (pthread_create(&recv_tid, NULL, start_receive_thread, NULL) != 0)
//     // {
//     //     perror("创建接收线程失败");
//     //     close(fd);
//     // }
//     // if(pthread_create(&monitor_tid, NULL, start_monitor_thread, NULL) != 0)
//     // {
//     //     perror("创建接受线程失败");
//     //     close(fd);
//     // }

//     // 等待线程结束
//     pthread_join(send_tid, NULL);
//     // pthread_join(recv_tid, NULL);
//     // pthread_join(monitor_tid, NULL);

// }

// void* Serial::send_thread(void *arg)
// {
//     VisionRecvStructTypedef message = {30.11, -15.11, 1, 100}; // 示例数据
//     unsigned char bufferW[sizeof(message) + 3];
//     while (1)
//     {
//         bufferW[0] = 0xFF; // 标识符
//         bufferW[1] = sizeof(message);
//         memcpy(bufferW + 2, &message, sizeof(message)); // 拷贝结构体数据
//         bufferW[sizeof(message) + 2] = 0xFE;
//         int n = write(fd, bufferW, sizeof(message) + 3);
//         if (n < 0)
//         {
//             perror("发送数据失败");
//             break;
//         }
//         else
//         {
//             printf("发送: Yaw = %f, Pitch = %f, Fire = %d, Distance = %d\n", message.serial_Yaw, message.serial_Pitch, message.fire, message.distance);
//             fflush(stdout);
//         }
//         usleep(100000); // 延时
//     }
//     exit(-1);
//     return nullptr;
// }

// void* Serial::receive_thread(void *arg) const
// {
//     unsigned char buffer[256];
//     while (keep_running)
//     {
//         // 读取数据，并检查是否至少包含头指针和结构体的大小
//         int nByte = read(fd, buffer, sizeof(buffer));
//         if (nByte >= sizeof(VisionRecvStructTypedef) + 2) // +1 是因为头指针
//         {
//             // 检查头指针是否为 0xFF
//             printf("接收串口工作中\n");
//             fflush(stdout);
//             if (buffer[0] == 0xFF)
//             {
//                 VisionRecvStructTypedef message;

//                 // 将数据拷贝到结构体
//                 memcpy(&message, buffer + 2, sizeof(VisionRecvStructTypedef));

//                 // 输出接收到的结构体数据
//                 printf("接收到数据: Yaw = %f, Pitch = %f, Fire = %d, Distance = %d\n",
//                 message.serial_Yaw, message.serial_Pitch, message.fire, message.distance);
//             }
//             else
//             {
//                 printf("错误：头指针不匹配，数据无效\n");
//             }
//         }
//         else if (nByte > 0)
//         {
//             printf("接收到的数据不完整\n");
//         }
//         else if (nByte < 0)
//         {
//             perror("读取错误");
//             break;
//         }
//     }
//     exit(-1);
//     return nullptr;
// }

// void * Serial::monitor_thread(void* arg)
// {
//     printf("按q退出程序...\n");
//     while(keep_running)
//     {
//         printf("检测执行中\n");
//         fflush(stdout);
//         char c = getchar();
//         if('q' == c || c == 'Q')
//         {
//             keep_running = 0;
//             int flags = fcntl(STDIN_FILENO, F_GETFL, 0);
//             fcntl(STDIN_FILENO, F_SETFL, flags | O_NONBLOCK);
//             printf("程序即将退出...\n");
//             break;
//         }
//         usleep(100000);
//     }
//     exit(-1);
//     return nullptr;
// }

// int Serial::set_opt(int fd, int nSpeed, int nBits, char nEvent, int nStop)
// {
//     struct termios newtio, oldtio;
//     if (tcgetattr(fd, &oldtio) != 0)
//     {
//         perror("无法获取串口属性");
//         return -1;
//     }
//     bzero(&newtio, sizeof(newtio));

//     newtio.c_cflag |= CLOCAL | CREAD;

//     newtio.c_cflag &= ~CSIZE;
//     switch (nBits)
//     {
//         case 7:
//             newtio.c_cflag |= CS7;
//             break;
//         case 8:
//         default:
//             newtio.c_cflag |= CS8;
//             break;
//     }

//     switch (nEvent)
//     {
//         case 'O':
//             newtio.c_cflag |= PARENB | PARODD;
//             newtio.c_iflag |= INPCK;
//             break;
//         case 'E':
//             newtio.c_cflag |= PARENB;
//             newtio.c_cflag &= ~PARODD;
//             newtio.c_iflag |= INPCK;
//             break;
//         case 'N':
//         default:
//             newtio.c_cflag &= ~PARENB;
//             break;
//     }

//     switch (nSpeed)
//     {
//         case 2400:
//             cfsetispeed(&newtio, B2400);
//             cfsetospeed(&newtio, B2400);
//             break;
//         case 4800:
//             cfsetispeed(&newtio, B4800);
//             cfsetospeed(&newtio, B4800);
//             break;
//         case 9600:
//             cfsetispeed(&newtio, B9600);
//             cfsetospeed(&newtio, B9600);
//             break;
//         case 115200:
//         default:
//             cfsetispeed(&newtio, B115200);
//             cfsetospeed(&newtio, B115200);
//             break;
//     }

//     if (nStop == 2)
//         newtio.c_cflag |= CSTOPB;
//     else
//         newtio.c_cflag &= ~CSTOPB;

//     newtio.c_cc[VTIME] = 0;
//     newtio.c_cc[VMIN] = 1;

//     tcflush(fd, TCIFLUSH);
//     if ((tcsetattr(fd, TCSANOW, &newtio)) != 0)
//     {
//         perror("设置串口属性失败");
//         return -1;
//     }
//     return 0;
// }

